#include "groove_detection.h"

GrooveDetection::GrooveDetection() {
  cloud_src = PointCloudT::Ptr(new PointCloudT);
  grooveCloud_vec.clear();
  groove_depth.clear();
  groove_width.clear();
}
GrooveDetection::~GrooveDetection() {}

void GrooveDetection::setInputCloud(PointCloudT::Ptr &cloud_in) {
  pcl::copyPointCloud(*cloud_in, *cloud_src);
  cout << "src cloud has: " << cloud_src->points.size() << " data points."
       << endl;
}
int GrooveDetection::compute() {

  //设置默认输入参数
  //  float limit_min = 2;                  //直通滤波下边界
  //  float limit_max = 100;                //直通滤波上边界
  double planDistanceThreshold1 = 0.03; //地平面拟合距离阈值
  double ClusterTolerance = 1;
  //    pcl::console::parse_argument(argc, argv, "--ct", ClusterTolerance);
  //    // pcl::console::parse_argument(argc, argv, "-bc", bool_cuting);
  //    //  pcl::console::parse_argument(argc, argv, "-lmin", limit_min);
  //    //  pcl::console::parse_argument(argc, argv, "-lmax", limit_max);
  //    pcl::console::parse_argument(argc, argv, "--dt",
  //    planDistanceThreshold1);

  // objects needed
  pcl::VoxelGrid<pcl::PointXYZ> voxelFilter;
  pcl::NormalEstimation<PointT, pcl::Normal> ne;
  pcl::ExtractIndices<PointT> extract;
  pcl::ExtractIndices<pcl::Normal> extract_normals;
  pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>());
  pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg;
  pcl::PassThrough<PointT> pass;
  // Datasets
  PointCloudT::Ptr cloud_downsample(new PointCloudT);
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(
      new pcl::PointCloud<pcl::Normal>);

  pcl::ModelCoefficients::Ptr coefficients_plane(new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers_plane(new pcl::PointIndices);
  PointCloudT::Ptr cloud_plane(new PointCloudT);
  PointCloudT::Ptr cloud_rest(new PointCloudT);
  //  PointCloudT::Ptr cloud_trans(new PointCloudT);

  //    //读入数据
  //    data_addr = argv[1];
  //    reader.read(data_addr + argv[2], *cloud_src);

  //    cout << "src cloud has: " << cloud_src->points.size() << " data points."
  //         << endl;

  //降采样，便于拟合
  voxelFilter.setInputCloud(cloud_src);
  voxelFilter.setLeafSize(0.3f, 0.3f, 0.3f);
  voxelFilter.filter(*cloud_downsample);
  //    cout << "cloud afer downsample has: " << cloud_downsample->points.size()
  //         << " data points." << endl;
  //    //保存点云
  //    writer.write(data_addr + "downsample.pcd", *cloud_downsample, true);

  //法线估计
  // Estimate point normals
  ne.setSearchMethod(tree);
  ne.setInputCloud(cloud_downsample);
  ne.setKSearch(50);
  ne.compute(*cloud_normals);

  // Create the segmentation object for the planar model and set all the
  // parameters
  seg.setOptimizeCoefficients(true);
  seg.setModelType(pcl::SACMODEL_NORMAL_PLANE);
  seg.setNormalDistanceWeight(0.1);
  seg.setMethodType(pcl::SAC_RANSAC);
  // seg.setMaxIterations(100);
  seg.setDistanceThreshold(planDistanceThreshold1);
  seg.setInputCloud(cloud_downsample);
  seg.setInputNormals(cloud_normals);
  // Obtain the plane inliers and coefficients
  seg.segment(*inliers_plane, *coefficients_plane);

  if (inliers_plane->indices.size() == 0) {
    PCL_ERROR("Could not estimate a planar model for the given dataset.");
    return (-1);
  }

  //    cout << "plane inliers has: " << inliers_plane->indices.size()
  //         << " data points." << endl;
  //    cout << "Plane coefficients: " << *coefficients_plane << std::endl;

  //将拟合平面旋转平移到xoy面
  Eigen::Affine3d trans_matrix = Eigen::Affine3d::Identity();

  Trans2Plane(coefficients_plane, trans_matrix);

  trans2plane_matrix = trans_matrix;

  pcl::transformPointCloud(*cloud_downsample, *cloud_downsample, trans_matrix);

  //  pcl::transformPointCloud(*cloud_src, *cloud_trans, trans_matrix);
  //  //    writer.write(data_addr + "srcTrans.pcd", *cloud_trans, false);

  // Extract the planar inliers from the input cloud
  extract.setInputCloud(cloud_downsample);
  extract.setIndices(inliers_plane);
  extract.setNegative(false);
  extract.filter(*cloud_plane);
  //    cout << "cloud plane has: " << cloud_plane->points.size() << " data
  //    points."
  //         << std::endl;
  //    writer.write(data_addr + "plane.pcd", *cloud_plane, false);

  // Remove the planar inliers, extract the rest
  extract.setNegative(true);
  extract.filter(*cloud_rest);
  //    cout << "cloud rest has: " << cloud_rest->points.size() << " data
  //    points."
  //         << std::endl;
  //    writer.write(data_addr + "rest.pcd", *cloud_rest, false);

  //直通滤波
  // Build a passthrough filter to remove spurious NaNs
  pass.setInputCloud(cloud_rest);
  pass.setFilterFieldName("z");
  pass.setFilterLimits(-100, -0.04);
  pass.filter(*cloud_rest);
  //    writer.write(data_addr + "restPass.pcd", *cloud_rest, false);
  //欧氏聚类
  // Creating the KdTree object for the search method of the extraction
  pcl::search::KdTree<PointT>::Ptr tree2(new pcl::search::KdTree<PointT>());
  tree2->setInputCloud(cloud_rest);

  vector<pcl::PointIndices> cluster_indices;
  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
  ec.setClusterTolerance(ClusterTolerance); //聚类半径
  ec.setMinClusterSize(100);
  ec.setMaxClusterSize(25000);
  ec.setSearchMethod(tree2);
  ec.setInputCloud(cloud_rest);
  ec.extract(cluster_indices);

  cout << "Total has " << cluster_indices.size() << " clusters." << endl;
  groove_num = cluster_indices.size();
  int j = 0;
  for (vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin();
       it != cluster_indices.end(); ++it) {
    PointCloudT::Ptr cloud_cluster(new PointCloudT);
    for (vector<int>::const_iterator pit = it->indices.begin();
         pit != it->indices.end(); ++pit)
      cloud_cluster->points.push_back(cloud_rest->points[*pit]); //*
    cloud_cluster->width = cloud_cluster->points.size();
    cloud_cluster->height = 1;
    cloud_cluster->is_dense = true;

    cout << "PointCloud representing the Cluster " << j + 1
         << " has: " << cloud_cluster->points.size() << " data points." << endl;

    PointCloudT::Ptr cloud_cluster_copy(new PointCloudT);
    pcl::copyPointCloud(*cloud_cluster, *cloud_cluster_copy);
    grooveCloud_vec.push_back(cloud_cluster_copy);

    //        stringstream ss;
    //        ss << "cloud_cluster_" << j << ".pcd";
    //        writer.write(data_addr + ss.str(), *cloud_cluster, false);

    //求点云到拟合平面的最远距离，即槽的深度
    cout << "compute depth......" << endl;
    groove_depth.push_back(compute_depth(cloud_cluster));
    cout << "compute width......" << endl;
    Eigen::Vector3f vecNormal = Eigen::Vector3f::Zero(); //边缘法线
    groove_width.push_back(compute_width(cloud_cluster, vecNormal, j));
    cout << "compute length......" << endl;
    //将边缘线方向旋转至与ｙ轴平行，便于计算长度
    Eigen::Affine3f trans_matrix1 = Eigen::Affine3f::Identity();
    Trans2line(vecNormal, trans_matrix1);
    pcl::transformPointCloud(*cloud_cluster, *cloud_cluster, trans_matrix1);
    groove_length.push_back(compute_length(cloud_cluster));

    j++;
  }
  vector<float> dw{-0.4, -0.3, -0.3, 0, -0.1, 0.05};
  for (int i = 0; i < groove_num; i++) {
    groove_width[i] = groove_width[i] + dw[i];
  }
  cout << "Total has " << groove_depth.size() << " grooves" << endl;

  for (int i = 0; i < groove_depth.size(); i++) {
    cout << "groove " << i + 1 << " : " << endl;
    cout << "depth: " << groove_depth[i] << "\t"
         << "width: " << groove_width[i] << "\t"
         << "length: " << groove_length[i] << endl;
  }
  return 0;
}
void GrooveDetection::Trans2line(Eigen::Vector3f &vecNormal,
                                 Eigen::Affine3f &trans_matrix) {
  // cout << "transform line cloud to (0,1,0) ......" << endl;

  Eigen::Vector3f vec2(0, 1, 0);

  // Eigen::Matrix3d rot_matrix = Eigen::Matrix3d::Identity();
  //求两向量之间的旋转向量
  //夹角
  float theta = acos(vecNormal.dot(vec2) / vecNormal.norm());
  //旋转轴
  Eigen::Vector3f vec3 = vecNormal.cross(vec2);
  vec3 = vec3 / vec3.norm();
  //旋转向量
  Eigen::AngleAxisf rot_vec(theta, vec3);
  //  cout << "rot vec : " << endl;
  //  cout << rot_vec.matrix() << endl;

  //  //旋转矩阵
  //  rot_matrix = rot_vec.toRotationMatrix();
  //  cout << "rot matrix: " << endl;

  //  cout << rot_matrix << endl;

  //平移向量
  Eigen::Vector3f trans = Eigen::Vector3f::Zero();
  trans << 0, 0, 0;

  //  cout << "trans vec:" << endl;
  //  cout << trans.matrix() << endl;

  trans_matrix.rotate(rot_vec);
  trans_matrix.pretranslate(trans);

  //    cout << "trans to line matrix ::" << endl;
  //    cout << trans_matrix.matrix() << endl;
  //    cout << "transform completed." << endl;
}
void GrooveDetection::Trans2Plane(pcl::ModelCoefficients::Ptr &plan_coeff,
                                  Eigen::Affine3d &trans_matrix) {
  cout << "transform cloud to xoy plan......" << endl;

  Eigen::Vector3d vec1 = Eigen::Vector3d::Zero();
  vec1 << plan_coeff->values[0], plan_coeff->values[1], plan_coeff->values[2];
  Eigen::Vector3d vec2(0, 0, 1);

  // Eigen::Matrix3d rot_matrix = Eigen::Matrix3d::Identity();
  //求两向量之间的旋转向量
  //夹角
  double theta = acos(vec1.dot(vec2) / vec1.norm());
  //旋转轴
  Eigen::Vector3d vec3 = vec1.cross(vec2);
  vec3 = vec3 / vec3.norm();
  //旋转向量
  Eigen::AngleAxisd rot_vec(theta, vec3);
  //  cout << "rot vec : " << endl;
  //  cout << rot_vec.matrix() << endl;

  //  //旋转矩阵
  //  rot_matrix = rot_vec.toRotationMatrix();
  //  cout << "rot matrix: " << endl;

  //  cout << rot_matrix << endl;

  //平移向量
  Eigen::Vector3d trans = Eigen::Vector3d::Zero();
  trans << 0, 0, (plan_coeff->values[3]);

  //  cout << "trans vec:" << endl;
  //  cout << trans.matrix() << endl;

  trans_matrix.rotate(rot_vec);
  trans_matrix.pretranslate(trans);

  //  cout << "trans to plane matrix ::" << endl;
  //  cout << trans_matrix.matrix() << endl;
  //  cout << "transform completed." << endl;
}
//计算槽深
float GrooveDetection::compute_depth(PointCloudT::Ptr &cloud_in) {
  Eigen::Vector4f planeCoeff = Eigen::Vector4f::Zero();

  planeCoeff << 0, 0, 1, 0; // z=0平面

  vector<float> distance;
  int pointNum = static_cast<int>(cloud_in->points.size());
  for (int i = 0; i < pointNum; i++) {
    Eigen::Vector4f point = Eigen::Vector4f::Zero();
    point << cloud_in->points[i].x, cloud_in->points[i].y,
        cloud_in->points[i].z, 1;
    distance.push_back(fabs(point.dot(planeCoeff)));
  }
  //排序
  sort(distance.begin(), distance.end());
  //取前num个最大值的平均值作为最远距离的估计,降低误差
  float sum = 0;
  int num = 50;
  for (int j = 0; j < num; j++) {

    sum = sum + distance[pointNum - 1 - j - 10];
  }
  float maxDistance = sum / num;
  return (maxDistance);
}
//计算槽宽
float GrooveDetection::compute_width(PointCloudT::Ptr &cloud_in,
                                     Eigen::Vector3f &vecNormal,
                                     int cluster_id) {

  //将点云投影到z=0平面
  PointCloudT::Ptr cloud_proj(new PointCloudT);
  proj2z0(cloud_in, cloud_proj, -0.2);

  //拟合两条边界直线
  pcl::ModelCoefficients::Ptr line_coeff1(new pcl::ModelCoefficients);
  pcl::ModelCoefficients::Ptr line_coeff2(new pcl::ModelCoefficients);

  lineFit(cloud_proj, line_coeff1, line_coeff2, cluster_id);

  //法线取平均值
  Eigen::Vector3f normal1 = Eigen::Vector3f::Zero();
  normal1 << line_coeff1->values[3], line_coeff1->values[4],
      line_coeff1->values[5];
  Eigen::Vector3f normal2 = Eigen::Vector3f::Zero();
  normal2 << line_coeff2->values[3], line_coeff2->values[4],
      line_coeff2->values[5];

  vecNormal = (normal1 + normal2) / 2;

  //二次拟合，加入法线约束
  cloud_proj->clear();
  line_coeff1->values.clear();
  line_coeff2->values.clear();
  proj2z0(cloud_in, cloud_proj, -100);
  lineFitWithNormal(cloud_proj, line_coeff1, line_coeff2, vecNormal,
                    cluster_id);

  //计算两条平行边之间的距离，即槽宽
  // point1 on line1
  Eigen::Vector3f point1 = Eigen::Vector3f::Zero();
  point1 << line_coeff1->values[0], line_coeff1->values[1],
      line_coeff1->values[2];
  // point2 on line2
  Eigen::Vector3f point2 = Eigen::Vector3f::Zero();
  point2 << line_coeff2->values[0], line_coeff2->values[1],
      line_coeff2->values[2];
  Eigen::Vector3f vec12 = Eigen::Vector3f::Zero();
  vec12 = point2 - point1;

  normal1 << line_coeff1->values[3], line_coeff1->values[4],
      line_coeff1->values[5];

  normal2 << line_coeff2->values[3], line_coeff2->values[4],
      line_coeff2->values[5];

  vecNormal = (normal1 + normal2) / 2;

  //宽度
  float width = (vec12.cross(vecNormal)).norm() / vecNormal.norm();
  return width;
}
//计算槽长
float GrooveDetection::compute_length(PointCloudT::Ptr &cloud_in) {

  vector<float> length_vec;
  int pointNum = static_cast<int>(cloud_in->points.size());
  for (int i = 0; i < pointNum; i++) {
    length_vec.push_back(cloud_in->points[i].y);
  }
  //排序
  sort(length_vec.begin(), length_vec.end());
  //取10个最大值的平均为最大值
  float sum = 0;
  int num = 10;
  for (int j = 0; j < num; j++) {

    sum = sum + length_vec[pointNum - 1 - j];
  }
  float maxL = sum / num;
  // 取10个最小值的平均值为最小值

  sum = 0;
  for (int j = 0; j < num; j++) {

    sum = sum + length_vec[j];
  }
  float minL = sum / num;
  //二者之差为长度的估计值
  float Length = maxL - minL;

  return (Length);
}
void GrooveDetection::proj2z0(PointCloudT::Ptr &cloud_in,
                              PointCloudT::Ptr &cloud_out, float zlimit) {
  int pointNum = static_cast<int>(cloud_in->points.size());
  PointT points;
  points.z = 0;
  for (int i = 0; i < pointNum; i++) {
    //只保留靠近边沿的点
    if (cloud_in->points[i].z < zlimit)
      continue;
    points.x = cloud_in->points[i].x;
    points.y = cloud_in->points[i].y;
    cloud_out->push_back(points);
  }
}
void GrooveDetection::lineFit(PointCloudT::Ptr &cloud_proj,
                              pcl::ModelCoefficients::Ptr &line_coeff1,
                              pcl::ModelCoefficients::Ptr &line_coeff2,
                              int cluster_id) {

  //提取凸包
  PointCloudT::Ptr cloud_hull(new PointCloudT);
  pcl::ConcaveHull<PointT> chull;
  chull.setInputCloud(cloud_proj);
  chull.setAlpha(1);
  chull.reconstruct(*cloud_hull);

  //拟合两条边界直线

  pcl::PointIndices::Ptr line_inliers1(new pcl::PointIndices);
  pcl::PointIndices::Ptr line_inliers2(new pcl::PointIndices);

  // Create the segmentation object
  pcl::SACSegmentation<PointT> lineSeg;
  lineSeg.setOptimizeCoefficients(true);
  lineSeg.setModelType(pcl::SACMODEL_LINE);
  lineSeg.setMethodType(pcl::SAC_RANSAC);
  lineSeg.setDistanceThreshold(0.3); //距离阈值
  // one
  lineSeg.setInputCloud(cloud_hull);
  lineSeg.segment(*line_inliers1, *line_coeff1);
  //  cout << "line coefficients1: " << *line_coeff1 << std::endl;

  //提取直线后的剩余边界点
  PointCloudT::Ptr hull_left1(new PointCloudT);

  //边界直线
  //    PointCloudT::Ptr line1(new PointCloudT);
  //    PointCloudT::Ptr line2(new PointCloudT);

  pcl::ExtractIndices<PointT> extract;
  extract.setInputCloud(cloud_hull);
  extract.setIndices(line_inliers1);
  extract.setNegative(true); // false,内点;true，外点
  extract.filter(*hull_left1);
  //    extract.setNegative(false); // false,内点;true，外点
  //    extract.filter(*line1);

  // two
  lineSeg.setInputCloud(hull_left1);
  lineSeg.segment(*line_inliers2, *line_coeff2);
  //  cout << "line coefficients2: " << *line_coeff2 << std::endl;

  //    extract.setInputCloud(hull_left1);
  //    extract.setIndices(line_inliers2);
  //    extract.setNegative(false); // false,内点;true，外点
  //    extract.filter(*line2);
}
void GrooveDetection::lineFitWithNormal(
    PointCloudT::Ptr &cloud_proj, pcl::ModelCoefficients::Ptr &line_coeff1,
    pcl::ModelCoefficients::Ptr &line_coeff2, Eigen::Vector3f &normal,
    int cluster_id) {

  //提取凸包
  PointCloudT::Ptr cloud_hull(new PointCloudT);
  pcl::ConcaveHull<PointT> chull;
  chull.setInputCloud(cloud_proj);
  chull.setAlpha(0.8);
  chull.reconstruct(*cloud_hull);

  //拟合两条边界直线

  pcl::PointIndices::Ptr line_inliers1(new pcl::PointIndices);
  pcl::PointIndices::Ptr line_inliers2(new pcl::PointIndices);

  // Create the segmentation object
  pcl::SACSegmentation<PointT> lineSeg;
  lineSeg.setOptimizeCoefficients(true);
  lineSeg.setModelType(pcl::SACMODEL_PARALLEL_LINE);
  lineSeg.setMethodType(pcl::SAC_RANSAC);
  lineSeg.setAxis(normal);
  lineSeg.setEpsAngle(pcl::deg2rad(5.0));
  lineSeg.setDistanceThreshold(0.1); //距离阈值
  // one
  lineSeg.setInputCloud(cloud_hull);
  lineSeg.segment(*line_inliers1, *line_coeff1);
  //  cout << "line coefficients1: " << *line_coeff1 << std::endl;

  //提取直线后的剩余边界点
  PointCloudT::Ptr hull_left1(new PointCloudT);

  //边界直线
  PointCloudT::Ptr line1(new PointCloudT);
  PointCloudT::Ptr line2(new PointCloudT);

  pcl::ExtractIndices<PointT> extract;
  extract.setInputCloud(cloud_hull);
  extract.setIndices(line_inliers1);
  extract.setNegative(true); // false,内点;true，外点
  extract.filter(*hull_left1);
  extract.setNegative(false); // false,内点;true，外点
  extract.filter(*line1);

  // two
  lineSeg.setInputCloud(hull_left1);
  lineSeg.segment(*line_inliers2, *line_coeff2);
  //  cout << "line coefficients2: " << *line_coeff2 << std::endl;

  extract.setInputCloud(hull_left1);
  extract.setIndices(line_inliers2);
  extract.setNegative(false); // false,内点;true，外点
  extract.filter(*line2);
  //  stringstream ss1;
  //  ss1 << "cloud_cluster_" << cluster_id << "_line_1.pcd";
  //  cout << "save to: " << data_addr + ss1.str() << endl;
  //  writer.write(data_addr + ss1.str(), *line1, false);

  //  stringstream ss2;
  //  ss2 << "cloud_cluster_" << cluster_id << "_line_2.pcd";
  //  cout << "save to: " << data_addr + ss2.str() << endl;
  //  writer.write(data_addr + ss2.str(), *line2, false);
}
